#include <cmath>
#include "C2DTurtle.h"

#define T2D_PI 3.14159265

C2DTurtle::C2DTurtle(float const step, float const lf)
    : turtle(),
      t_stack(),
      length_factor(lf)
{
    turtle.x_pos = turtle.y_pos = 0;
    turtle.angle_deg = 0;
    turtle.step_size = step;
}

void C2DTurtle::MoveForward()
{
    float angle_rad = turtle.angle_deg * T2D_PI / float(180.0);
    turtle.x_pos += turtle.step_size * std::cos(angle_rad);
    turtle.y_pos += turtle.step_size * std::sin(angle_rad);
}

void C2DTurtle::Turn(float const angle)
{
    turtle.angle_deg += angle;
    if(turtle.angle_deg > 360.0) turtle.angle_deg -= 360.0;
    else
        if(turtle.angle_deg < 0.0) turtle.angle_deg += 360.0;
}

void C2DTurtle::PushState(void)
{
    t_stack.push(turtle);
}

void C2DTurtle::PopState(void)
{
    turtle = t_stack.top();
    t_stack.pop();
}
